#!/usr/bin/env python3
# -*- coding: UTF-8 -*-

import rospy
from geometry_msgs.msg import PoseStamped

robot_name = ["tbmn_01", "tbmn_02"]


def pose_callback_01(msg):
        sx = msg.pose.position.x
        sy = msg.pose.position.y
        sz = msg.pose.position.z
        rospy.loginfo("\r\nsub " + robot_name[0] + " position \
                        \r\n[x: %.2f, y: %.2f, z: %.2f]" %
                        (sx, sy, sz))

def pose_callback_02(msg):
        sx = msg.pose.position.x
        sy = msg.pose.position.y
        sz = msg.pose.position.z
        rospy.loginfo("\r\nsub " + robot_name[1] + " position \
                        \r\n[x: %.2f, y: %.2f, z: %.2f]" %
                        (sx, sy, sz))

def pose_subscribe():
    rospy.Subscriber('/' + robot_name[0] + '_pose', PoseStamped, pose_callback_01, queue_size=10)
    rospy.Subscriber('/' + robot_name[1] + '_pose', PoseStamped, pose_callback_02, queue_size=10)


if __name__ == '__main__':
    try:
        rospy.init_node('pose_subscribe', anonymous=True)
        pose_subscribe()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
